% Mobile rotation
clear;
clc;

qcpp = load('Mobile_rotation.txt'); % C++/asm rotation

s = size(qcpp);
n = s(1);

dt = load('dt.txt');
q = [1 0 0 0]; % Matlab rotation
L = [0.716956948242946 -0.648748537522841 0.0480028688888];
torque = [0.015 0.667 -0.333];
Ibody = [1.6666667e+004  0.0000000e+000  0.0000000e+000;  0.0000000e+000  1.6666667e+004  0.0000000e+000;  0.0000000e+000  0.0000000e+000  1.6666667e+004];
Ibodyinv = inv(Ibody);
error = zeros(n, 4);

for i=1:n
    R = quat2mat(q);
    Rt = R';
    L = L + torque * dt;
    Iinv = R * Ibodyinv * Rt;
    omega = L * Iinv;
    dq = .5 * quatmultiply([0 omega], q);
    q = q + dq*dt;
    
    q = quatnormalize(q);
    
    error(i,:) = q - qcpp(i,:);
end

% final Matlab rotation
q

figure(1);
clf;
hold on;
plot(error(:,1)', 'b');
plot(error(:,2)', 'r');
plot(error(:,3)', 'c');
plot(error(:,4)', 'y');
title({'Mobile object rotation';'(rounding mode: NEAR)'});
xlabel(['iteration (dt = ', num2str(dt, '%.16f'), 's)']);
ylabel('error');
legend('s', 'vx', 'vy', 'vz');
